// Devin Koepl

#ifndef FUNCS_H_DISCRETIZE
#define FUNCS_H_DISCRETIZE

#define		DISCRETIZE(VAL, VAL_MIN, VAL_MAX, COUNT_MIN, COUNT_MAX) \
	((uint32_t)((VAL - VAL_MIN) / (VAL_MAX - VAL_MIN) * ((double)COUNT_MAX - (double)COUNT_MIN) + (double)COUNT_MIN))

#define		UNDISCRETIZE(COUNT, VAL_MIN, VAL_MAX, COUNT_MIN, COUNT_MAX) \
	(((float)COUNT - (float)COUNT_MIN) * (VAL_MAX - VAL_MIN) / ((float)COUNT_MAX - (float)COUNT_MIN) + VAL_MIN)

// inputs:
//	COUNT			- raw input from sensor
//	COUNT_KNOWN		- raw sensor value corresponding to known angle
//	ANGLE_KNOWN		- the known angle in radians
//	COUNTS_PER_REV	- the number of counts per full rotation of the encoder shaft
//	GEAR_RATIO		- the ratio between the encoder shaft and the final output
// outputs an floating point value representing the angle.
#define		UNDISCRETIZE_LOCATION(COUNT, COUNT_KNOWN, ANGLE_KNOWN, COUNTS_PER_REV, GEAR_RATIO) \
	(1./(float)(GEAR_RATIO) * 2. * PI / (float)COUNTS_PER_REV * ((float)COUNT - (float)COUNT_KNOWN) + (float)ANGLE_KNOWN)

#endif // FUNCS_H_DISCRETIZE
